#参数介绍
# H1x和H1y,H1vx,H1vy,H1ax,H1ay分别为不同时刻观测到的目标的x和y坐标，速度以及加速度，由csv文件输入
# X_0和 X_0分别为利用二次曲线拟合的得到的预测的目标的x和y坐标
# X_和 X_分别为利用ca模型的得到的预测的目标的x和y坐标X
# F为利用ca模型的得到的预测的目标的x和y坐标，速度以及加速度
# X0和Y0分别为不同时刻用曲线拟合预测目标轨迹的x和y坐标
# # X和Y,VX,VY,AX,AY分别为不同时刻用来预测目标轨迹的x和y坐标，速度以及加速度
#  所需库
import csv
import yaml
import numpy as np

# 选择评估方式，当选择曲线拟合时，m = 0，name2 = "1"，为ca模型时，m = 1，name2 = "2"
m = 0
name2 = "1"

# 获取当前csv文件和其有效行数
infile = input('请输入文件路径：\n')
# infile = '/home/fanwei/桌面/SAIC/raw data/object1-1-test.csv'
n = sum(1 for line in open(infile)) - 1
# 获取输出yaml输出路径和名字
path = __file__
for i in range(len(path) -1, -1, -1):
    if infile[i] == "/":
        l0 = i+1
        break
for i in range(len(infile) -1, -1, -1):
    if infile[i] == "/":
        l1 = i+1
        break
name0 = path[0:l0]
name1 = infile[l1:-8]
name3="-problem frame.yml"
outfile = name0 + name1 + name2 + name3
# 选择阈值选取方式并设置阈值
t = 0
#选择比例阈值，并设定阈值
if t == 0:
    Tx0 = 0.1
    Ty0= 0.1
#选择绝对阈值方式，并设定阈值
elif t == 1:
    Tx1 = 1
    Ty1= 1

# 计算预测二次轨迹系数函数
# 生成系数矩阵A
def gen_coefficient_matrix(X, Y):
    N = len(X)
    m = 3
    A = []
    # 计算每一个方程的系数
    for i in range(m):
        a = []
        # 计算当前方程中的每一个系数
        for j in range(m):
            a.append(sum(X ** (i+j)))
        A.append(a)
    return A
# 计算方程组的右端向量b
def gen_right_vector(X, Y):
    N = len(X)
    m = 3
    b = []
    for i in range(m):
        b.append(sum(X**i * Y))
    return b

# 构造ca（恒定加速度模型），输入前一帧的位置,速度以及加速度，返回下一帧的位置,速度以及加速度
def camodel(x,vx,ax,y,vy,ay):
    dt = 0.1 # Time Step between Filter Steps
    m =3
    X = np.zeros((6,m))#状态值X = [x,vx,ax,y,vy,ay],6个值
    X0 = np.mat([[x,vx,ax,y,vy,ay]])
    A = np.matrix([[1,dt,0.5*dt*dt,0,0,0],
                [0, 1,dt, 0, 0, 0],
                [0, 0,1, 0, 0, 0],
                [0, 0, 0, 1,dt,0.5*dt*dt],
                [0, 0, 0, 0,1, dt],
                [0, 0, 0, 0,0, 1]])
    X[:,0] = X0
    for i in range(1,m):
        X[:,i] = (A*X0.T).T
        X0 = np.mat(X[:,i])
    return( X[:,1])

    # 读取csv文件并分别构造H1x,H1X,H1vx,H1vy,H1ax,H1ay
with open(infile,'rt') as csvfile1:
    reader1 = csv.reader(csvfile1)
    rows1 = [row for row in reader1]
    H1 = np.array(rows1)
    (H1x ,H1y,H1vx,H1vy,H1ax,H1ay ) = ([],[],[],[],[],[])
    for i in range(n):
        H1x.append(H1[i+1,2])
        H1y.append(H1[i+1,3])
        H1vx.append(H1[i+1,4])
        H1vy.append(H1[i+1,5])
        H1ax.append(H1[i+1,6])
        H1ay.append(H1[i+1,7])
# 当采用曲线拟合方法进行评估时：
if m == 0:
    (X0,Y0,T0) = ([],[],[])
    for i in range(5):
        T0.append(i/10)
        X0.append(float(H1x[i]))
        Y0.append(float(H1y[i]))
    (X0,Y0,T0) = (np.array(X0),np.array(Y0),np.array(T0))
    A = gen_coefficient_matrix(T0, X0)
    a = gen_right_vector(T0, X0)
    a0, a1, a2 = np.linalg.solve(A, a)
    B = gen_coefficient_matrix(T0, Y0)
    b = gen_right_vector(T0,Y0)
    b0, b1,b2 = np.linalg.solve(B, b)
    T_0 = np.arange(0, 10.1, 0.1)
    X_0 = np.array([a0 + a1*t + a2*t**2 for t in T_0])
    Y_0 = np.array([b0 + b1*t + b2*t**2 for t in T_0])
    with open(outfile , "w") as output_stream:
        yaml.dump(rows1[0], output_stream,default_flow_style=True)
    for i in range(5,n):#从第5帧开始判断
    # 根据阈值类型进行场景判断
        if t == 0:
            if abs((float(H1x[i]) - float(X_0[i]))/float(X_0[i])) <= Tx0 and abs((float(H1y[i]) - float(Y_0[i]))/float(Y_0[i]))  <= Ty0:#作差与比例阈值比较进行判断
                s0 =0 
            else:
                s0 = 1
        elif t == 1:
            if abs(float(H1x[i]) - float(X_0[i]) )<= Tx1 and abs((float(H1y[i]) - float(Y_0[i])))  <= Ty1:#作差与绝对阈值比较进行判断
                s0 = 2
            else:
                s0 = 3
    # 当没出现问题帧时，将观测数据值 H1x，H1y写入X，Y
        if s0 == 0 or s0 == 2:
            (X0,Y0,T0) = (list(X0),list(Y0),list(T0))
            X0.append(float(H1x[i]))
            Y0.append(float(H1y[i]))
            T0.append((i)/10)
            if i > 30:#最多使用历史30帧数据进行曲线拟合
                del(X0[0],Y0[0],T0[0])
    #    当出现问题帧时，剔除问题帧数据，将预测数据值X_，Y_写入X，Y
        else:
            (X0,Y0,T0) = (np.array(X0),np.array(Y0),np.array(T0))
            A = gen_coefficient_matrix(T0, X0)
            a = gen_right_vector(T0, X0)
            a0, a1, a2 = np.linalg.solve(A, a)
            B = gen_coefficient_matrix(T0, Y0)
            b = gen_right_vector(T0,Y0)
            b0, b1,b2 = np.linalg.solve(B, b)
            (X0,Y0,T0) = (list(X0),list(Y0),list(T0))
            X_0 = np.array([a0 + a1*t + a2*t**2 for t in T_0])
            Y_0 = np.array([b0 + b1*t + b2*t**2 for t in T_0])
            X0.append(float(X_0[i]))
            Y0.append(float(Y_0[i]))
            T0.append((i)/10)
            if i >30:#最多使用历史30帧数据进行曲线拟合
                del(X0[0],Y0[0],T0[0])
    #将问题帧写入yml文件
            with open(outfile , "a") as output_stream:
                yaml.dump(rows1[i+1], output_stream,default_flow_style=True) 

# 当采用曲线拟合方法进行评估时：
elif m == 1:
    (X,Y,T,VX,VY,AX,AY) = ([],[],[],[],[],[],[])
    for i in range(5):
        T.append(i/10)
        X.append(float(H1x[i]))
        Y.append(float(H1y[i]))
        VX.append(float(H1vx[i]))
        VY.append(float(H1vy[i]))
        AX.append(float(H1ax[i]))
        AY.append(float(H1ay[i]))
    (X,Y,T,VX,VY,AX,AY) = (list(X),list(Y),list(T),list(VX),list(VY),list(AX),list(AY))
#  构造初始 yml文件
    with open(outfile, "w") as output_stream:
        yaml.dump(rows1[0], output_stream,default_flow_style=True)

    # 对每帧进行检查，剔除问题帧中的问题数据，并输出问题帧。使用ca模型进行拟合。
    for i in range(5,n):
# 获取该预测所需的过去5帧的加速度平均值和上一帧的速度与坐标 
        ax = 0.2*(float(AX[i-1])+float(AX[i-2])+float(AX[i-3])+float(AX[i-4])+float(AX[i-5]))#计算上五帧的及速度作为ca模型的加速度输出
        ay = 0.2*(float(AY[i-1])+float(AY[i-2])+float(AY[i-3])+float(AY[i-4])+float(AY[i-5]))#计算上五帧的及速度作为ca模型的加速度输出
        F =  camodel(float(X[i-1]),float(VX[i-1]),ax,float(Y[i-1]),float(VY[i-1]),ay)
        # 获取利用ca模型的到目标坐标x和坐标y
        x_ =  float(F[0])
        y_ = float(F[3])
        # 获取当前的观测到的目标坐标x和坐标y
        _x = float(H1x[i])
        _y = float(H1y[i])
#对二者进行阈值求差 
# # 根据阈值类型进行场景判断
        if t == 0:
            if  (abs(_x - x_))/x_ <= Tx0 and  (abs (_y - y_ ))/x_ <= Ty0:#作差与比例阈值比较进行判断
                s =0 
            else:
                s = 1
        elif t == 1:
            if  abs(_x - x_) <= Tx1 and  abs (_y - y_)  <= Ty1:#作差与绝对阈值比较进行判断
                s = 2
            else:
                s = 3
  # 当没出现问题帧时，将观测数据值 H1x,H1y,H1vy,H1ax,H1ay写入X,,Y,VX,VY,AX,AY
        if  s == 0 or s == 2:
            (X,Y,T) = (list(X),list(Y),list(T))
            T.append(i/10)
            X.append(float(H1x[i]))
            Y.append(float(H1y[i]))
            VX.append(float(H1vx[i]))
            VY.append(float(H1vy[i]))
            AX.append(float(H1ax[i])) 
            AY.append(float(H1ay[i]))
#   当出现问题帧时，剔除问题帧数据，将预测数据值写入X,,Y,VX,VY,AX,AY
        else:
            X.append(float(F[0]))
            Y.append(float(F[3]))
            VX.append(float(F[1]))
            VY.append(float(F[4]))
            AX.append(float(F[2]))
            AY.append(float(F[5]))
        #将问题帧写入yml文件
            with open(outfile, "a") as output_stream:
                yaml.dump(rows1[i+1], output_stream,default_flow_style=True)